#!/usr/bin/env python
#coding=utf-8
#move lib
import rospy
from geometry_msgs.msg import Twist
import time
import roslaunch
import thread

#navigation lib
import actionlib
from actionlib_msgs.msg import *
from geometry_msgs.msg import Pose, Point, Quaternion, Twist
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from tf.transformations import quaternion_from_euler
from math import pi

#nlp lib
from aip import AipSpeech 
import os 

#vision lib
import cv2 
from cv_bridge import CvBridge
from sensor_msgs.msg import Image
import pyzbar.pyzbar as pyzbar
import sys

#arm lib
from aikit_arm.srv import claw

class move():
    def __init__(self,mytopic="cmd_vel"):
        rospy.init_node('robotbase', anonymous=True)
        self.topic=mytopic
        self.pub = rospy.Publisher(self.topic, Twist, queue_size=10)
        self.rate = rospy.Rate(10) # 10hz
        self.cmd_vel=Twist()
        
    def turn(self,turn_speed=0,times=0):   
        t=time.time()
        self.cmd_vel.linear.x=0
        self.cmd_vel.angular.z=turn_speed
        while not rospy.is_shutdown() and (time.time()-t)<times:
            self.pub.publish(self.cmd_vel)
            self.rate.sleep()
    def go(self,go_speed=0,times=0):
        t=time.time()
        self.cmd_vel.linear.x=go_speed
        self.cmd_vel.angular.z=0
        while not rospy.is_shutdown() and (time.time()-t)<times:
            self.pub.publish(self.cmd_vel)
            self.rate.sleep()

class nav():
    def __init__(self):
        # rospy.init_node('nav_demo',anonymous=False) #初始化节点
        self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction) #实例化action服务
        rospy.loginfo('等待move_base action服务器连接...')
        self.move_base.wait_for_server(rospy.Duration(30)) #等待连接服务
        rospy.loginfo('已连接导航服务')  #提示连接成功

    def nav_to(self,x,y,d):#导航函数，添加坐标点,输入x（前）坐标，y（左）坐标，th（平面朝向-360～360度）
        goal = MoveBaseGoal()  #实例化目标点消息
        goal.target_pose.header.frame_id='map'  #设置目标tf
        goal.target_pose.header.stamp = rospy.Time.now()  #获取系统时间戳
        goal.target_pose.pose.position.x=float(x)  #设置x坐标
        goal.target_pose.pose.position.y=float(y)  #设置y坐标
        quaternion=quaternion_from_euler(0.0,0.0,float(d)/180.0*pi) #根据传入欧拉角d转四元数
        goal.target_pose.pose.orientation.x=quaternion[0] #四元数x赋值
        goal.target_pose.pose.orientation.y=quaternion[1] #四元数y赋值
        goal.target_pose.pose.orientation.z=quaternion[2] #四元数z赋值
        goal.target_pose.pose.orientation.w=quaternion[3] #四元数w赋值
        rospy.loginfo('尝试导航去坐标点:('+str(x)+","+str(y)+','+str(d)+')')  #提示连接成功
        self.move_base.send_goal(goal) #发送导航目标
        finished_within_time = self.move_base.wait_for_result(rospy.Duration(10))  #等待10秒，获取导航结果
        if not finished_within_time: #如果导航失败
            self.move_base.cancel_goal() #取消导航
            rospy.loginfo('时间超时，导航任务取消。') #打印取消导航
            return 0
        state = self.move_base.get_state() #获取导航状态
        if state == GoalStatus.SUCCEEDED: #如果导航状态为SUCCEED
            rospy.loginfo('导航成功！') #提示导航成功
            return 1
    def cancel_goal():
        self.move_base.cancel_goal()      

class nlp():
    def __init__(self):
        self.APP_ID = '16425806'  
        self.API_KEY = 'Lz5VMGB5qufER9kxiik0IR1a'
        self.SECRET_KEY = 'EgGkUUHWHLtkkYG2GIpAswG9hK7hOzMH'
        self.client = AipSpeech(self.APP_ID,self.API_KEY,self.SECRET_KEY) 
    
    def tts(self,words,language="zh",speed=5,pit=5,person=0,volume=10,):
	    result= self.client.synthesis(words, language, 1, {
	        'spd': speed,   #语速 0最小 9最大
	        'pit': pit,    #音调 0最小 9最大   
	        'vol': volume,  #音量 0最小 15最大
	        'per': person   # 发音人 0：女声 1：男声 3:度逍遥 4：度丫丫
	        })

	    if not isinstance(result, dict):
		    file_name = "/home/robot/Music/tts_temp.mp3"     
		    with open(file_name, 'wb') as f:  
			    f.write(result) 
		    os.system('play '+file_name)


class arm():
    def __init__(self):
        rospy.wait_for_service("claw")
        rospy.loginfo('已连接机械臂控制服务')
        self.claw = rospy.ServiceProxy("claw", claw)
    def setclaw(self,pos):
        self.claw(pos)

    







